#include <assert.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <ctype.h>
#include <time.h>
#include "config.h"
#include "random.h"
#include "matrix.h"
#include "kdtree.h"

#define DEF_NUM_PTS 10



/* get a random double between -10 and 10 */
typedef math::random_generator<double, math::uniform_tag, -10, 10> random_type ;
typedef math::kdtree <double , 3> tree_type ;
typedef math::kdres <double, 3> res_type ;
typedef math::vector <double, 3> point_type ;

random_type generator ;
int main(int argc, char **argv) {
	int i, num_pts = DEF_NUM_PTS;

	unsigned *data, *pch;
	res_type *presults;
	double dist;

	point_type pt , pos ;
	pt = 0, 0, 1; 
	double radius = 10;

	if(argc > 1 && isdigit(argv[1][0])) {
		num_pts = atoi(argv[1]);
	}

	if(!(data = (unsigned *)malloc(num_pts * sizeof (unsigned)))) {
		perror("malloc failed");
		return 1;
	}

	// srand( time(0) );
	generator.seed(10);
	tree_type tree ;
  
	/* add some random nodes to the tree (assert nodes are successfully inserted) */
	for( i=0; i<num_pts; i++ ) {
		data[i] = i;
		assert( 0 == tree.kd_insert( point_type (generator.random(), generator.random(), generator.random() ), &data[i] ) );
	}

	presults = tree.kd_nearest_range(  pt, radius , 1);
	printf( "found %d results:\n", presults->kd_res_size() );
	while( !presults->kd_res_end() ) {
		pch = (unsigned *)presults->kd_res_item(pos );
		dist = distance (pt, pos); 
		printf( "node at (%.3f, %.3f, %.3f) is %.3f away and has data=%u\n", 
				pos[0], pos[1], pos[2], dist, *pch );
		presults->kd_res_next();
	}

	presults->kd_res_free();
	free(presults);

	pch = (unsigned *)tree.kd_nearest(pt, pos); 

	dist = distance (pt, pos) ;
	printf( "node at (%.3f, %.3f, %.3f) is %.3f away and has data=%u\n", 
			pos[0], pos[1], pos[2], dist, *pch );


	free( data );
	return 0;
}
